% ************************************************************************ 
%     Module name   : Lynx Simulation - World Run function                                                   
%                                                                          
%     Description   : This will step time in the World.                                                    
%                                                                          
%     $Header: /Lynx/SimulationFramework/RSP/IFS/IFSRun.m 2     15/10/08 16:08 Lwabeke $ 
%                                                                          
%    $Author: Lwabeke $	            
%                                                                         
%    $Revision: 2 $                
%                                                                         
% ************************************************************************
%     $History: IFSRun.m $ 
% 
%
% ************************************************************************
function [ currentTime] = World_Run(new_time,  param)
global worldState;

persistent max_delta_time;

if (isempty(max_delta_time ))
    max_delta_time = 0;
end


worldState.activeTargets = 0;

oldestActive = 0;

for loopCntr=1:2
    
    if (loopCntr==1)
        startCntr=1;
        stopCntr=worldState.numTargets;
    else
        startCntr=worldState.oldestActive;
    end
    
    tgtCntr = startCntr;
    
    while ((loopCntr == 1)&&(tgtCntr <= stopCntr)) ...
            ||((loopCntr == 2)&&(tgtCntr < size(worldState.Target,2))&&(worldState.Target(tgtCntr).initTime < new_time))
        
        Target = worldState.Target(tgtCntr);
        
        if (new_time < Target.initTime)
            % Not visible yet, only useful for targets, clutter sorting and
            % sliding window should handle this case
        elseif (new_time < Target.endTime)
            if ((loopCntr == 2)&&(oldestActive == 0))
                oldestActive = tgtCntr;   % Keeps track of the oldest clutter index that was found to still be active
            end
            
            %count active targets
            worldState.activeTargets = worldState.activeTargets +1;
            
            delta_time = min([new_time - Target.initTime ; new_time - worldState.currentTime]);
            
            if (delta_time > max_delta_time)
                max_delta_time = delta_time;  % Longest timestep we have had
            end
            
            switch (Target.type)
                case 0  % Target velocity converge wind velocity, requires delta_time < 1
                    worldState.Target(tgtCntr).currentVel = (Target.currentVel * (1-delta_time) + worldState.wind * delta_time );
                    worldState.Target(tgtCntr).currentPos = Target.currentPos +  Target.currentVel * delta_time;

                case 1  
                    % Target movement caused own propulsion, wind and limited by drag 
                    % steering towards end point
                    
                    delta_time_remaining = delta_time;
                    
                    while (delta_time_remaining > 0)
                        if delta_time_remaining > 0.5
                            delta_time = 0.5;
                        else
                            delta_time = delta_time_remaining;
                        end
                        delta_time_remaining = delta_time_remaining - delta_time;    
                        Target = worldState.Target(tgtCntr);
                        
                        v = Target.currentVel;
                        a = [Target.currentProp 0];
                        heading = Target.currentHeading;
                        
                        theta = [ sin(heading) cos(heading) ; cos(heading) -sin(heading) ];
                        
                        worldState.Target(tgtCntr).currentPos = Target.currentPos +  v * delta_time;
                        
                        worldState.Target(tgtCntr).currentVel = v + (worldState.wind * Target.sailfactor + a * theta - (v * v')^0.5 * v * theta * param.world.Drag * theta) * delta_time;
                        
                        delta_dist = Target.endPos - Target.currentPos;
                        worldState.Target(tgtCntr).currentHeading = atan2(delta_dist(1), delta_dist(2));
                        
                        if (sum(isnan(worldState.Target(tgtCntr).currentVel))>0)
                            Target.currentPos
                        end
                    
                    end

                case 2  
                    % requires delta_time < 1 or risks becoming unstable
                    % Target movement caused own propulsion, wind and limited by drag 
                    % Slowly turning ships heading at constant rate
                    v = Target.currentVel;
                    a = [Target.currentProp 0];
                    heading = Target.currentHeading;
                    
                    theta = [ sin(heading) cos(heading) ; cos(heading) -sin(heading) ];
                    
%                    worldState.Target(tgtCntr).currentVel = v + (worldState.wind * Target.sailfactor + a * theta - (v * v')^0.5 * v * theta * param.world.Drag * theta) * delta_time;
                    newVel = v + (worldState.wind * Target.sailfactor + a * theta - (v * v')^0.5 * v * theta * param.world.Drag * theta) * delta_time;
                    
%                    if (isnan(newVel))
%                     if (newVel * newVel') > 100
%                         new_time
%                         (newVel * newVel')
%                         pause
%                     end
                    worldState.Target(tgtCntr).currentVel = newVel;
                    
                    worldState.Target(tgtCntr).currentPos = Target.currentPos +  v * delta_time;
                    
                    worldState.Target(tgtCntr).currentHeading = Target.currentHeading + Target.currentTurnRate * delta_time;
                    
            end
        elseif (Target.endTime < new_time)
            worldState.Target(tgtCntr).currentPos = Target.endPos;
        end
        
        %    worldState.Target(tgtCntr) = Target;
        
        
        
        %     if (new_time < worldState.Target(tgtCntr).initTime)
        %         % Not visible yet
        %     elseif (new_time < worldState.Target(tgtCntr).endTime)
        %         %count active targets
        %         worldState.activeTargets = worldState.activeTargets +1;
        %
        %         delta_time = min([new_time - worldState.Target(tgtCntr).initTime ; new_time - worldState.currentTime]);
        %         worldState.Target(tgtCntr).currentPos = worldState.Target(tgtCntr).currentPos + worldState.Target(tgtCntr).currentVel * delta_time;
        % %        Target.currentVel = (Target.endPos - Target.initPos) / (Target.endTime - Target.initTime);
        %     elseif (worldState.Target(tgtCntr).endTime < new_time)
        %         worldState.Target(tgtCntr).currentPos = worldState.Target(tgtCntr).endPos;
        %     end
       
        tgtCntr = tgtCntr + 1;
    end
end

if  (oldestActive > 0)
    worldState.oldestActive  = oldestActive;
end

%  if (new_time > 8000)
%      worldState.wind = 0 * worldState.wind;
%  end

worldState.currentTime = new_time;
currentTime = new_time;

